#include "rads_business.hpp"
std::mutex g_db_mutex;
// 自动驾驶
void radarThreadFunction(std::vector<car> &pathPoints)
{
    double distance;
    double NDT_JDC;
    double NDT_JDC_new;
    int speed = SPEED;                        // 初始速度
    int maxspeed = MAXSPEED;                    // 最大数度
    int minspeed = SPEED;
    int speed_new;
    int api_change_counter_update=0;
    int speed_change_counter_update=0;
    //吸附点
    int snapped_index = findSnappedWaypoint(pathPoints);
    if (snapped_index != -1) {
        const car& target_point = pathPoints[snapped_index];
        std::cout << "当前吸附到点位: [" << target_point.num_point << "] ("<< target_point.x << ", " << target_point.y << ")" << std::endl;
    } else {
        std::cout << "未找到有效吸附点" << std::endl;
    }

    // for (auto &point : pathPoints) {
    //     // 处理每个点位
    //     std::cout << "前往点位: (" << point.num_point << "," << point.x << ", " << point.y << ")" << std::endl;
    // }
    while (snapped_index < pathPoints.size() && !stopRadarThread.load()){
        api_change_counter_update++;
        speed_change_counter_update++;
        // 1. 获取当前目标点
        const car& target_point = pathPoints[snapped_index];
        // 2. 输出导航信息
        std::cout << "总共："<< pathPoints.size() << ",导航至点位 [" << target_point.num_point << "]: (" 
        << target_point.x << ", " << target_point.y << ")" 
        << " | 距离:" << distance_count(lidar_rtk.x, lidar_rtk.y, 
                                       target_point.x, target_point.y)
        << "米" << std::endl;
        //计算车到目标点的距离
        distance = distance_count(lidar_rtk.x, lidar_rtk.y, target_point.x, target_point.y);
        if (distance < DRIVE_DISRANCE) {
            std::cout << "已到达点位 [" << target_point.num_point << "]" << std::endl;
            snapped_index++; // 移向下一个点
        }
        if (api_change_counter_update >= API_CHANGE_INTERVAL)
            {
                api_change_counter_update = 0; // 重置计数器 
                publishSradsackProgress(snapped_index,pathPoints,speed_new);
            }
        //计算角度并发送话题给
        NDT_JDC = calculate_NDT_JDC(target_point);
            //std::cout << "NDT_JDC:" << NDT_JDC << std::endl; 

        if (stop_car == 1) // 正常驾驶
        {
                
               while ((Stop_lidar == 1 || ladar_5 < FLLR || ladar_6 < FLLR) && !stopRadarThread.load())
                {
                    pubdrive(0, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
                    std::cout << ",前方有障碍，正在停车，当前速度: " << 0 << " KM/h" << std::endl;  
                     speed = SPEED;                        // 初始速度
                    usleep(20000);
                }
                   // 减速
                if (ladar_1 < LFRFLRRR || ladar_2 < LFRFLRRR || ladar_3 < LFRFLRRR || ladar_4 < LFRFLRRR)
                {
                    std::cout << "车辆正在无人驾驶,收到减速信号"<< std::endl;
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed > minspeed)
                        {
                            speed = speed - 1;
                        }
                    }
                }else{
                    // 加速--》直到最大速度
                    if (speed_change_counter_update >= SPEED_CHANGE_INTERVAL)
                    {
                        speed_change_counter_update = 0; // 重置计数器
                        if (speed < maxspeed)
                        {
                            speed = speed + 1;
                        }
                    }
                }



                if(std::abs(NDT_JDC)   <  30 &&  std::abs(NDT_JDC)  > 0){
                      NDT_JDC_new=NDT_JDC * 1.5;
                 }else if(std::abs(NDT_JDC)   <  40 &&  std::abs(NDT_JDC)  >= 30){
                    NDT_JDC_new =NDT_JDC * 1.2;
                 }else if(std::abs(NDT_JDC)   > 60){
                     NDT_JDC_new =NDT_JDC * 0.3;
                 }else{
                    NDT_JDC_new=NDT_JDC;
                 }
                speed_new = cos(NDT_JDC_new/114.6) * speed;
                if(std::abs(NDT_JDC)   > 60){
                    speed_new =3;
                }
                pubdrive(0, 2, NDT_JDC_new* 100 + 18000.0, speed_new);  
        }else{

          pubdrive(0, 2, 18000, 0);
        }
      
        usleep(20000);
        //std::cout << "前往点位: (" << pathPoints[snapped_index].num_point << "," <<pathPoints[snapped_index].x << ", " << pathPoints[snapped_index].y << ")" << std::endl;
        //snapped_index++;
    }
    rads::rads_acks msg;
    msg.ack = 3;
    int baifenbiend = 100;
    int lichengend = 0;
    int duoshaomiaoend = 0;
    std::string progressStr = std::to_string(baifenbiend) + "," + std::to_string(lichengend) + "," + std::to_string(duoshaomiaoend);
    msg.progress = progressStr;
    sradsack_pub_.publish(msg);
    std::cout << "正常--无人驾驶结束" << std::endl;
    pubdrive(100, 2, 18000, 0); // int brake, int gear, int steering, int target_speed
}
// 返航驾驶
void thread_Create_line_goback_play(int map_id)
{
}
// 马踏飞燕
void thread_Create_line_all(int map_id)
{
    int caidiannum_point = 1;
    double last_ndt_x, last_ndt_y;
    while (should_run_all.load())
    {
        double cai_dis = distance_count(lidar_rtk.x, lidar_rtk.y, last_ndt_x, last_ndt_y);
        if (cai_dis > AUTO_DISRANCE)
        {

            last_ndt_x = lidar_rtk.x;
            last_ndt_y = lidar_rtk.y;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 888;
            rads_acks_msg.map_id = map_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate=lidar_rtk.matching_rate;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建基本路线,在地图号：" << map_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建基本路线,在地图号：" << map_id << ",一共：" << caidiannum_point - 1 << "个点" << endl;
}
// go_back 采集点
void thread_Create_line_goback(int map_id)
{
    // 清空表
    delete_go_back();
    sleep(1);
    // 发布话题
    int caidiannum_point = 1;
    double last_ndt_x, last_ndt_y;
    // 原地第一个点
    while (should_run_goback.load())
    {
        double cai_dis = distance_count(lidar_gobackmsg.x, lidar_gobackmsg.y, last_ndt_x, last_ndt_y);
        if (cai_dis > AUTO_DISRANCE)
        {
            last_ndt_x = lidar_gobackmsg.x;
            last_ndt_y = lidar_gobackmsg.y;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.cmd = 999;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_gobackmsg.x;
            trajectory_ext.y = lidar_gobackmsg.y;
            trajectory_ext.z = lidar_gobackmsg.z;
            trajectory_ext.yaw = lidar_gobackmsg.ld_heading;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建返航路线,,踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建返航路线,一共：" << caidiannum_point << "个点" << endl;
}
// 踩点带路线和地图
void thread_Create_line(int map_id, int path_id, string pro_nema)
{
    double last_x,last_y,last_heading,current_x,current_y,current_heading,distance,heading_diff;
    bool initial_use_rtk,use_rtk;
    int caidiannum_point = 1;
    initial_use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
    last_x = initial_use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
    last_y = initial_use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
    last_heading = initial_use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;

    while (should_run.load())
    {
        use_rtk = (lidar_rtk.matching_rate < 55) && (lidar_rtk.posi_flag == 4) && (lidar_rtk.head_flag == 4);
        
        current_x = use_rtk ? lidar_rtk.rtk_x : lidar_rtk.x;
        current_y = use_rtk ? lidar_rtk.rtk_y : lidar_rtk.y;
        current_heading = use_rtk ? lidar_rtk.heading : lidar_rtk.ld_heading;
        distance = distance_count(current_x, current_y, last_x, last_y);
        heading_diff = normalized_heading_diff(current_heading, last_heading);

        if (distance > AUTO_DISRANCE || heading_diff > HEADING_THRESHOLD)
        {
            last_x = current_x;
            last_y = current_y;
            last_heading = current_heading;
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = pro_nema;
            rads_acks_msg.cmd = 12;
            rads_acks_msg.cmd_opt = map_id;
            rads_acks_msg.ext1 = path_id;
            rads_acks_msg.ext2 = 1;
            rads_acks_msg.ack = 2;
            rads_acks_msg.map_id = map_id;
            rads_acks_msg.path_id = path_id;
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = caidiannum_point;
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.rtk_x;
            trajectory_ext.rtk_y = lidar_rtk.rtk_y;
            trajectory_ext.rtk_z = lidar_rtk.rtk_z;
            trajectory_ext.yaw = lidar_rtk.ld_heading;
            trajectory_ext.ld_heading = lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
            cout << "创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",踩点成功！当前点个数: " << caidiannum_point << endl;
            caidiannum_point++;
        }
        usleep(100);
    }
    cout << "结束创建路线,在地图号：" << map_id << " ,创建了线路号：" << path_id << ",一共：" << caidiannum_point-1 << "个点" << endl;
}
// 接受网络指令
void pradscmdCallback(const rads::rads_cmd::ConstPtr &msg)
{
    sradsack_fast sradsack_fast_data;
    sradsack_fast_data.pro_nema = msg->pro_nema;
    sradsack_fast_data.cmd = msg->pro_cmd;
    sradsack_fast_data.cmd_opt = msg->pro_cmd_opt;
    sradsack_fast_data.ext1 = msg->pro_ext1;
    sradsack_fast_data.ext2 = msg->pro_ext2;

    ROS_INFO("Received pradscmd message:");
    ROS_INFO_STREAM("pro_nema: " << msg->pro_nema);
    ROS_INFO_STREAM("pro_head: " << msg->pro_head);
    ROS_INFO_STREAM("pro_vehsn: " << msg->pro_vehsn);
    ROS_INFO_STREAM("pro_cmd: " << msg->pro_cmd);
    ROS_INFO_STREAM("pro_cmd_opt: " << msg->pro_cmd_opt);
    ROS_INFO_STREAM("pro_ext1: " << msg->pro_ext1);
    ROS_INFO_STREAM("pro_ext2: " << msg->pro_ext2);
    for (int i = 0; i < msg->data.size(); ++i)
    {
        ROS_INFO_STREAM("data[" << i << "]: " << msg->data[i]);
    }
    // 任意点导航
    if (msg->pro_cmd == 30)
    {
        std::cout << "***********************" << "任意点导航开始" << "*****************************" << std::endl;
        // 地图    msg->pro_cmd_opt;
        // 点数    msg->pro_ext1;
        // msg->pro_ext2  0 ：预览  1：执行   2：执行带返航
        rads::rads_acks acksmsg;
        rads::rads_trackext trajectory_ext;
        acksmsg.header.stamp = ros::Time::now();
        acksmsg.header.frame_id = "sradsack_map";
        acksmsg.pro_nema = msg->pro_nema;
        acksmsg.cmd = msg->pro_cmd;
        acksmsg.cmd_opt = msg->pro_cmd_opt;
        acksmsg.ext1 = msg->pro_ext1;
        acksmsg.ext2 = msg->pro_ext2;
        acksmsg.ack = 2;
        acksmsg.map_id = msg->pro_cmd_opt;
        double x1, y1, x2, y2;
        std::vector<std::vector<int16_t>> all_paths;
        all_paths.clear();
        if (msg->pro_ext2 != 0) // 预览不清除表
        {
           // 清空表
        }
        //获取所有基本点
        auto pathPoints = fetchPointallDataFromDB(msg->pro_cmd_opt);

        if (msg->pro_ext1 > 0 && msg->data.size() >= msg->pro_ext1 * 2) // 执行
        {
            x1 = lidar_rtk.x;
            y1 = lidar_rtk.y;
            x2 = msg->data[0] * 0.01;
            y2 = msg->data[1] * 0.01;
            all_paths.push_back(getPath_anypoint(x1, y1, x2, y2,pathPoints));
            for (int i = 0; i < (msg->pro_ext1 - 1) * 2; i += 2)
            {
                x1 = msg->data[i] * 0.01;
                y1 = msg->data[i + 1] * 0.01;
                x2 = msg->data[i + 2] * 0.01;
                y2 = msg->data[i + 3] * 0.01;
                all_paths.push_back(getPath_anypoint(x1, y1, x2, y2,pathPoints));
            }
        }

         // 遍历 all_paths 并打印每个路径点
         int num = 1;
         for (size_t i = 0; i < all_paths.size(); ++i)
         {
             for (size_t j = 0; j < all_paths[i].size(); ++j)
             {
                 auto it = std::find_if(pathPoints.begin(), pathPoints.end(), [&](const point_all &p)
                                        { return p.num == all_paths[i][j] + 1; });
                 if (it != pathPoints.end())
                 {
                     const point_all &pointall = *it;
                     trajectory_ext.point_count = num;
                     trajectory_ext.alltotal=all_paths[i].size();
                     trajectory_ext.x = pointall.x;
                     trajectory_ext.y = pointall.y;
                     trajectory_ext.z = pointall.z;
                     trajectory_ext.ld_heading = pointall.lidar_heading;
                     trajectory_ext.matching_rate = pointall.ndt_matching;
                     trajectory_ext.rtk_x = pointall.rtk_x;
                     trajectory_ext.rtk_y = pointall.rtk_y;
                     trajectory_ext.rtk_z = pointall.rtk_z;
                     trajectory_ext.longitude = pointall.lng;
                     trajectory_ext.latitude = pointall.lat;
                     trajectory_ext.heading = pointall.rtk_heading;
                     trajectory_ext.posi_flag = pointall.position_flag;
                     trajectory_ext.head_flag = pointall.orientation_flag;
                     acksmsg.trajectory_ext.push_back(trajectory_ext);
                     num++;
                   
                 }
             }
         }
         sradsack_pub_.publish(acksmsg);

         stopRadarThread.store(true);
         sleep(2);
       // 获取该路径上的所有点位
        auto point_data_points = fetchPointdataDataFromDB();
       // 启动自动驾驶线程
       stopRadarThread.store(false);
       std::thread radarThread(radarThreadFunction, std::ref(point_data_points));
       radarThread.detach();
    }


    // 创建基本线路（马踏飞燕）
    if (msg->pro_cmd == 7)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline_all.joinable())
        {
            std::cout << "***********************" << "创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            should_run_all.store(true);
            thread_instance_createline_all = std::thread(std::bind(thread_Create_line_all, msg->pro_cmd_opt));
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }

        if (msg->pro_ext2 == 0 && thread_instance_createline_all.joinable())
        {
            should_run_all.store(false);
            if (thread_instance_createline_all.joinable())
            {
                thread_instance_createline_all.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建基本线路,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 终止任务
    if (msg->pro_cmd == 20)
    {
        std::cout << "***********************" << "终止任务" << "*****************************" << std::endl;
        stopRadarThread.store(true); // 发送停止信号
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.ack = 1;
        publishacks(sradsack_pub_, sradsack_fast_data);
    }

    // 调度路线
    if (msg->pro_cmd == 5)
    {
        std::cout << "**********去往" << msg->pro_cmd_opt << "号地图," << msg->pro_ext1 << "号线的" << msg->pro_ext2 << "号点位***************" << std::endl;
        // 获取该路径上的所有点位
        auto pathPoints = fetchCarDataFromDB(msg->pro_cmd_opt, msg->pro_ext1);
        sradsack_fast_data.ack = 1;
        sradsack_fast_data.map_id = msg->pro_cmd_opt;
        sradsack_fast_data.path_id = msg->pro_ext1;
        publishacks(sradsack_pub_, sradsack_fast_data);
        stopRadarThread.store(true);
        sleep(2);
       // 启动自动驾驶线程
       stop_car = 1;
       stopRadarThread.store(false);
       std::thread radarThread(radarThreadFunction, std::ref(pathPoints));
       radarThread.detach();
    }

    // 临时停车
    if (msg->pro_cmd == 8)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "临时停车" << "*****************************" << std::endl;
            stop_car = 2;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除临时停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 紧急制动停车
    if (msg->pro_cmd == 9)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "紧急制动停车" << "*****************************" << std::endl;
            stop_car = 3;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "解除紧急制动停车" << "*****************************" << std::endl;
            stop_car = 1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建线路
    if (msg->pro_cmd == 12)
    {
        if (msg->pro_ext2 == 1 && !thread_instance_createline.joinable())
        {

            std::cout << "***********************" << "创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
            should_run.store(true);
            thread_instance_createline = std::thread(std::bind(thread_Create_line, msg->pro_cmd_opt, msg->pro_ext1, msg->pro_nema));
        }
        if (msg->pro_ext2 == 0 && thread_instance_createline.joinable())
        {
            should_run.store(false);
            if (thread_instance_createline.joinable())
            {
                thread_instance_createline.join(); // 等待线程结束
            }
            std::cout << "***********************" << "结束创建线路,地图编号:" << msg->pro_cmd_opt << "线路编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.path_id = msg->pro_ext1;
            sradsack_fast_data.ack = 1;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 设置开机点
    if (msg->pro_cmd == 16)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "上传开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            // 上报
            rads::rads_acks rads_acks_msg;
            rads_acks_msg.header.stamp = ros::Time::now();
            rads_acks_msg.pro_nema = msg->pro_nema;
            rads_acks_msg.cmd = msg->pro_cmd;
            rads_acks_msg.cmd_opt = msg->pro_cmd_opt;
            rads_acks_msg.ext1 = msg->pro_ext1;
            rads_acks_msg.ext2 = msg->pro_ext2;
            rads_acks_msg.ack = 1;
            rads_acks_msg.map_id = msg->pro_cmd_opt;
            nav_msgs::Path trajectory;
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = lidar_rtk.x;
            pose.pose.position.y = lidar_rtk.y;
            pose.pose.position.z = lidar_rtk.z;
            pose.pose.orientation.x = lidar_rtk.ox;
            pose.pose.orientation.y = lidar_rtk.oy;
            pose.pose.orientation.z = lidar_rtk.oz;
            pose.pose.orientation.w = lidar_rtk.ow;
            rads_acks_msg.trajectory.poses.push_back(pose);
            rads::rads_trackext trajectory_ext;
            trajectory_ext.point_count = msg->pro_ext1; // 开机点编号
            trajectory_ext.x = lidar_rtk.x;
            trajectory_ext.y = lidar_rtk.y;
            trajectory_ext.z = lidar_rtk.z;
            trajectory_ext.rtk_x = lidar_rtk.x;
            trajectory_ext.rtk_y = lidar_rtk.y;
            trajectory_ext.rtk_z = lidar_rtk.z;
            trajectory_ext.ld_heading=lidar_rtk.ld_heading;
            trajectory_ext.matching_rate = lidar_rtk.matching_rate;
            trajectory_ext.longitude = lidar_rtk.longitude;
            trajectory_ext.latitude = lidar_rtk.latitude;
            trajectory_ext.heading = lidar_rtk.heading;
            trajectory_ext.posi_flag = lidar_rtk.posi_flag;
            trajectory_ext.head_flag = lidar_rtk.head_flag;
            rads_acks_msg.trajectory_ext.push_back(trajectory_ext);
            sradsack_pub_.publish(rads_acks_msg);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "删除开机点，地图编号:" << msg->pro_cmd_opt << "开机点编号：" << msg->pro_ext1 << "*****************************" << std::endl;
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
    }
    // 创建地图
    if (msg->pro_cmd == 10)
    {
        if (msg->pro_ext2 == 1)
        {
            std::cout << "***********************" << "开始建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(10);
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            publishacks(sradsack_pub_, sradsack_fast_data);
        }
        if (msg->pro_ext2 == 0)
        {
            std::cout << "***********************" << "结束建图,地图编号:" << msg->pro_cmd_opt << "*****************************" << std::endl;
            char command[256];
            snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./endtu.sh %d &", (int)msg->pro_cmd_opt);
            int result = system(command);
            sleep(25);
            std::string map_path = "/home/argo/catkin_ws/pcd/" + std::to_string(msg->pro_cmd_opt) + ".pcd";
            sradsack_fast_data.ack = 1;
            sradsack_fast_data.map_id = msg->pro_cmd_opt;
            sradsack_fast_data.map_path=map_path;
            publishacks(sradsack_pub_, sradsack_fast_data);
            map_ck(msg->pro_cmd_opt);

        }
    }
}
// 接受融合后的雷达+RTK数据
void laserCallback(const rads::rads_trackext::ConstPtr &msg)
{
    // 获取雷达数据
    lidar_rtk.x = isnan(msg->x) ? 0.0 : msg->x;
    lidar_rtk.y = isnan(msg->y) ? 0.0 : msg->y;
    lidar_rtk.z = isnan(msg->z) ? 0.0 : msg->z;
    lidar_rtk.ox = isnan(msg->ox) ? 0.0 : msg->ox;
    lidar_rtk.oy = isnan(msg->oy) ? 0.0 : msg->oy;
    lidar_rtk.oz = isnan(msg->oz) ? 0.0 : msg->oz;
    lidar_rtk.ow = isnan(msg->ow) ? 0.0 : msg->ow;
    // 处理四元数数据
    lidar_rtk.ld_heading = quaternionToEulerAngle(msg->ox, msg->oy, msg->oz, msg->ow);
    // 获取RTK数据
    // lidar_rtk.rtk_x = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    // lidar_rtk.rtk_y = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    double x1 = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    double y1 = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    double yaw = isnan(msg->heading) ? 0.0 : msg->heading;
    double baseline = RTK_DISRANCE;
    double xMid, yMid;
    calculateMidPoint(x1, y1, yaw, baseline, xMid, yMid);
    lidar_rtk.rtk_x = isnan(yMid) ? 0.0 : yMid;
    lidar_rtk.rtk_y = isnan(xMid) ? 0.0 : xMid;


    lidar_rtk.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    lidar_rtk.heading = isnan(msg->heading) ? 0.0 : msg->heading;
    lidar_rtk.longitude = isnan(msg->latitude) ? 0.0 : msg->latitude;
    lidar_rtk.latitude = isnan(msg->longitude) ? 0.0 : msg->longitude;
    lidar_rtk.posi_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    lidar_rtk.head_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    lidar_rtk.groud_speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
}
// 避障（停）
void points_rawCallback(const sensor_msgs::PointCloud2ConstPtr &msg_pc)
{
    pcl::PointCloud<pcl::PointXYZ> pc;
    pcl::fromROSMsg(*msg_pc, pc);
    int Stop_3Dpoint = 0;
    for (const pcl::PointXYZ &p : pc.points)
    {
        if (p.x <= 0.8 && p.x >= -0.8 && p.z >= -0.3 && p.y >= 0.1 && p.y <= 1.5) // 停障区(p.x)/激光雷达左右宽度,(p.y);//激光雷达前后(p.z);//激光雷达高度差
        {
            Stop_3Dpoint++;
        }
    }
    //std::cout << "Stop_3Dpoint:" << Stop_3Dpoint << std::endl; 
    if (Stop_3Dpoint > 300)
    {
        Stop_lidar = 1; // 遇到障碍停车
    }else{
        Stop_lidar = 0; // 没有障碍正常跑
    }
}
// 下位机来的数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    int model = msg->model; // （1:遥控，2:跟随和推，3:返航，4:无人驾驶）
    if (model != 4)
    {                                // 模式不等于4，
        stopRadarThread.store(true); // 发送停止信号
    }
    // 记忆按下键值为1，开始记忆路线
    if (model == 2 && !thread_instance_createline_goback.joinable())
    {

        char command[256];
        snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./starttu.sh %d &", 1);
        int result = system(command);
        sleep(2);
        int map_id = 0;
        std::cout << "***********************" << "创建goback线路" << "*****************************" << std::endl;
        should_run_goback.store(true);
        thread_instance_createline_goback = std::thread(std::bind(thread_Create_line_goback, map_id));
    }
    // 结束记忆路线，并结束记忆路线线程。
    if (model != 2 && thread_instance_createline_goback.joinable())
    {
        should_run_goback.store(false);
        if (thread_instance_createline_goback.joinable())
        {
            thread_instance_createline_goback.join(); // 等待线程结束
        }
        std::cout << "***********************" << "结束创建goback线路" << "*****************************" << std::endl;
    }

    // 首次触发返航开启返航线程，返航按下值为1.
    if (model == 3 && !thread_instance_createline_goback_play.joinable())
    {
        int map_id = 0;
        std::cout << "***********************" << "返航开始" << "*****************************" << std::endl;
        should_run_goback_play.store(true);
        thread_instance_createline_goback_play = std::thread(std::bind(thread_Create_line_goback_play, map_id));
    }
    // 记忆弹起键值为0，结束返航线程并清空线路
    if (model != 3 && thread_instance_createline_goback_play.joinable())
    {
        should_run_goback_play.store(false);
        if (thread_instance_createline_goback_play.joinable())
        {
            thread_instance_createline_goback_play.join(); // 等待线程结束
        }
        std::cout << "***********************" << "遥控---结束返航" << "*****************************" << std::endl;
    }
}
// 接受goback定位
void odometryCallgoback(const nav_msgs::Odometry::ConstPtr &msg)
{

    lidar_gobackmsg.x = msg->pose.pose.position.x;
    lidar_gobackmsg.y = msg->pose.pose.position.y;
    lidar_gobackmsg.z = msg->pose.pose.position.z;
    lidar_gobackmsg.ld_heading = quaternionToEulerAngle(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
}
// 接受imu  sensor_msgs::Imu
void callbackimu(const sensor_msgs::Imu &msg)
{
    imu_msg.x = msg.orientation.x;
    imu_msg.y = msg.orientation.y;
    imu_msg.z = msg.orientation.z;
}
int main(int argc, char **argv)
{
    if(RTK_LIKE==1){
     //   int cc = 1232;
    map_ck(getmapid());
    //cout << getmapid() << endl;
    } 
    ros::init(argc, argv, "radar_business");
    ros::NodeHandle nh_;
    // 订阅网络发来指令
    static ros::Subscriber sub_ = nh_.subscribe("pradscmd", 1000, pradscmdCallback);
    // 订阅融合后的雷达+RTK数据
    static ros::Subscriber ndt_pose = nh_.subscribe("pradsfusion", 1, laserCallback); // ndt_pose
    // 订阅points_raw 用于激光雷达停障
    static ros::Subscriber sub_points_raw = nh_.subscribe<sensor_msgs::PointCloud2>("/points_raw", 100, points_rawCallback);
    // 订阅下位机发来的指令 pradsscmd
    static ros::Subscriber sub_pradsscmd = nh_.subscribe("pradsscmd", 1, pradsscmdCallback);
    // 订阅返航定位
    static ros::Subscriber sub_pradsscmd_goback_weizhi = nh_.subscribe("/odometry/imu", 1, odometryCallgoback);
    // sensor_msgs::Imu
    static ros::Subscriber imusub = nh_.subscribe("imu603", 1, callbackimu);
    // 发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）
    sradsack_pub_ = nh_.advertise<rads::rads_acks>("sradsack", 1000);
    // 发布控制车辆的信息
    sradsdrive = nh_.advertise<rads::rads_scmdret>("sradsdriveackermann", 10);
    // 发布地图重定位
    initialposepub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
    ros::spin();
    return 0;
}